/*
 * @Author: huangyupei huangyupei2021@ia.ac.cn
 * @Date: 2024-09-23 14:37:22
 * @LastEditors: huangyupei huangyupei2021@ia.ac.cn
 * @LastEditTime: 2024-11-27 16:38:35
 * @FilePath: /SelfCalib_OptiAcoustic/include/g2oTypes.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#pragma once

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <opencv2/opencv.hpp>

// #include <g2o/core/base_vertex.h>
// #include <g2o/core/base_binary_edge.h>
// #include <g2o/core/block_solver.h>
// #include <g2o/core/optimization_algorithm_levenberg.h>
// #include <g2o/solvers/dense/linear_solver_dense.h>
// #include <g2o/solvers/csparse/linear_solver_csparse.h>
// #include <g2o/core/robust_kernel_impl.h>
// #include <g2o/types/sba/types_six_dof_expmap.h>

#include "Thirdparty/g2o/g2o/core/base_vertex.h"
#include "Thirdparty/g2o/g2o/core/base_binary_edge.h"
#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/types/types_sba.h"

#include "sophus/se3.hpp"
#include "camera_pose_estimation.h"


using namespace std;
using namespace Eigen;
using namespace Sophus;
using namespace cv;

// g2o自带的三维位姿顶点类型： g2o::VertexSE3Expmap 类型，不过还是得自己实现一遍，需要顺便实现 sonar_project 函数
// class VertexSE3Expmap: public g2o::BaseVertex<6, SE3Quat>

// g2o自带的三维点顶点类型：VertexPointXYZ   VertexSBAPointXYZ
// 下面是SLAM十四讲 ch9 代码中定义的三维点类型
// ~ 需要注意的是, 在声呐BA中，三维点用球坐标表示（!2024年11月1日15:51:44 更新：用直角坐标表示了~！）
// class VertexPoint: public g2o::BaseVertex<3, Vector3d> {
// public:
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

//     VertexPoint() {}
//     virtual void setToOriginImpl() override {
//         _estimate = Vector3d(0, 0, 0);
//     }

//     virtual void oplusImpl(const double *update_) override {
//         Eigen::Map<const Eigen::Vector3d> update(update_);
//         _estimate += update;
//         // _estimate[2] += update[2];
//     }

//     virtual bool read(istream &in) {}
//     virtual bool write(ostream &out) const {}
// };


// 将point表示的3D点(表示在世界系下)投影到vSE3代表的坐标系下, vSE3 is 该坐标系在世界系下的表示
Eigen::Vector2d SonarProject(g2o::SE3Quat vSE3, const Vector3d &point) {
    Sophus::SE3d T_s1_s2(vSE3.rotation(), vSE3.translation());
    Sophus::SE3d T_s2_s1 = T_s1_s2.inverse();
    
    // ~ 声呐投影函数的实现，3D点为球坐标形式    
    // double r = point[0], theta = point[1], phi = point[2];
    // Sophus::Vector3d P_s1_j(r * sin(theta) * cos(phi), r * cos(theta) * cos(phi), r * sin(phi));
    // Sophus::Vector3d P_s2_j = T_s2_s1 * P_s1_j;
    // double x = P_s2_j[0], y = P_s2_j[1], z = P_s2_j[2];
    // return Vector2d(sqrt(x*x + y*y + z*z), atan2(x, y));

    // ~ 声呐投影函数的实现，3D点为直角坐标形式
    Sophus::Vector3d P_s2_j = T_s2_s1 * point;
    double Xs = P_s2_j[0], Ys = P_s2_j[1] ,Zs = P_s2_j[2];
    double r = sqrt(Xs*Xs + Ys*Ys + Zs*Zs), theta = atan2(Xs, Ys);
    return Vector2d(r * sin(theta), r*cos(theta));
}

Eigen::Vector2d SonarProjection(const Vector3d &point) {
    // ~ 声呐投影函数的实现，3D点为直角坐标形式
    double Xs = point[0], Ys = point[1], Zs = point[2];
    double r = sqrt(Xs*Xs + Ys*Ys + Zs*Zs), theta = atan2(Xs, Ys);
    return Vector2d(r * sin(theta), r*cos(theta));
}

class EdgeSonarProjection: public g2o::BaseBinaryEdge<2, Vector2d, g2o::VertexSE3Expmap, VertexPoint> {
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        auto v0 = (g2o::VertexSE3Expmap *) _vertices[0];        // T_s1_s2, 声呐位姿在第一帧声呐坐标系下的表示
        auto v1 = (VertexPoint *) _vertices[1];                 // 3D Point，表示在第一帧坐标系下
        auto proj = SonarProject(v0->estimate(), v1->estimate());
        _error = proj - _measurement;
    }

    // 使用数值求导，暂时不计算雅克比矩阵
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}   
};

class EdgeLocalSonarBAProjection: public g2o::BaseBinaryEdge<2, Vector2d, VertexPose, VertexPoint> {
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        auto v0 = (VertexPose *) _vertices[0];        // T_s1_s2, 声呐位姿在第一帧声呐坐标系下的表示
        auto v1 = (VertexPoint *) _vertices[1];                 // 3D Point，表示在第一帧坐标系下
        Sophus::SE3d T_si_s0 = v0->estimate();
        Vector3d P_s0_j = v1->estimate();

        Vector3d P_si_j = T_si_s0 * P_s0_j;
        auto proj = SonarProjection(P_si_j);
        _error = _measurement - proj;
    }

    // 使用数值求导，暂时不计算雅克比矩阵
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}   
};

class EdgePnpSonarProjection: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgePnpSonarProjection(const Eigen::Vector3d &pos) : _pos3d(pos) {}

    virtual void computeError() override {
        auto v0 = (g2o::VertexSE3Expmap *) _vertices[0];        // T_s1_si, 声呐位姿在第一帧声呐坐标系下的表示
        auto proj = SonarProject(v0->estimate(), _pos3d);
        _error = proj - _measurement;
    }

    // 使用数值求导，暂时不计算雅克比矩阵
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}   
private:
    Eigen::Vector3d _pos3d;         // pos: 3D Point，表示在第一帧坐标系下
};



/**
 * @brief 声呐两视图BA优化问题
 * 
 * @param[in] T_s1_s2 第二帧声呐位姿，传入初值，优化后传出优化值
 * @param[in] vPoints 直角坐标形式的三维点坐标，表示在第一帧声呐坐标系下，传入初值，传出优化后的值
 * @param[in] vSonarMeas1 第一帧声呐产生的测量
 * @param[in] vSonarMeas2 第二帧声呐产生的测量
 */
void SolveAcousticBA(Sophus::SE3d &T_s1_s2, vector<Vector3d> &vPoints, vector<Vector2d> vSonarMeas1, vector<Vector2d> vSonarMeas2, bool priorZ_positive);

/**
 * @brief 声呐的PnP问题的优化解法
 * 
 * @param[in] T_s1_si 第i帧声呐位姿，传入初值，优化后传出优化值
 * @param[in] vPoints 直角坐标形式的3D点坐标，固定
 * @param[in] vSonarMeasi 第i帧声呐位姿处的2D测量，用于构建重投影误差
 */
void AcousticPnpBA(Sophus::SE3d &T_s1_si, vector<Vector3d> vPoints, vector<Vector2d> vSonarMeasi);


/**
 * @brief 声呐的局部BA函数，待优化的是直角坐标形式的3D点、声呐位姿序列T_s1_si，观测量是每个位姿处的声呐2D测量
 * 
 * @param[in] vPointsSonar 
 * @param[in] vSonarPoses 
 * @param[in] vSonarMeasAll 
 */
void LocalSonarBA(vector<Vector3d> &vPointsSonar, vector<Sophus::SE3d> &vSonarPoses, vector<vector<Vector2d>> vSonarMeasAll);